#include "root.h"



int csb_data_write(UCHAR itf,UCHAR addr,UCHAR cmd,USHORT staraddr,USHORT num,unsigned char *buf,int maxlen)
{
	UCHAR *pkt = (UCHAR*)FAAL_SNDBUF(itf);
	rtu_pkt_t *rtu = (rtu_pkt_t*)FAAL_RCVBUF(itf);
	UCHAR uc;
	int len;
	int retry = 0;
	len = modbus_write(addr,cmd,staraddr,num,maxlen,buf,pkt);
	if(len > 0)
	{
		//if(FAAL_NEEDPRINT(itf)) faal_print645pkt(itf,pkt, len,"SEND");	

		while(!(*g_faalitf[itf].getchar)(&uc)) os_sleep(1);	

		if((*g_faalitf[itf].rawsend)(pkt, len)) return -1;

		while(retry < 4)
		{
			if(!faal_modbus_rcvpkt(itf))
			{
				len = data_update(rtu,addr,cmd,buf,maxlen);
				
				return len;
			}
			os_sleep(5); //50
			retry++;
		}
	//	if(FAAL_NEEDPRINT(itf)) faal_print645pkt(itf,pkt,0,"timeout");		
	}
	return -1;
}


unsigned short Read_ultrasound_distance(void)
{
    int len;
    int ul;
		static unsigned short distance = 0;
	
		static unsigned int err_cnt = 0;
	
    UCHAR buf[10] ={0};

		UART_ReInit(UART3_CZ, 9600, UART_DEF_FRAME_N, UART_DEF_MODE);
		os_sleep(5);
    len = csb_data_write(FAALITF_CZ,0x01,READ_HOLD,0x0102,1,buf,10);
    if(len > 0)
    {
			  distance = (buf[0] << 8) | buf[1];
				err_cnt = 0;
				return distance;
    }
		
		return 2550;
		
}